import rclpy
from .fusion import SensorFusion
from .ros_interface import SensorFusionROSInterface

def main(args=None):
    rclpy.init(args=args)
    
    # ROS接口
    ros_interface = SensorFusionROSInterface()
    
    # 传感器融合算法
    sensor_fusion = SensorFusion()
    
    # 设置回调函数
    ros_interface.set_gps_callback(sensor_fusion.process_gps)
    ros_interface.set_imu_callback(sensor_fusion.process_imu)
    ros_interface.set_odom_callback(sensor_fusion.process_odom)
    
    def publish_fused_pose():
        state = sensor_fusion.get_current_state()
        ros_interface.publish_fused_pose(state)
    
    timer = ros_interface.create_timer(0.05, publish_fused_pose)  # 20Hz发布频率
    
    # 开始处理
    try:
        rclpy.spin(ros_interface)
    except KeyboardInterrupt:
        pass
    finally:
        ros_interface.destroy_timer(timer)
        ros_interface.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()    
